#include <cmath>
#include "point.hpp"

/** @brief Constructor
    @param x The initial value for the x component
    @param y The initial value for the y component
    @param z The initial value for the z component
*/
util::point::point(double x, double y, double z) {
  data[0] = x;
  data[1] = y;
  data[2] = z;
}

/** @brief Destructor */
util::point::~point() {}

/** @brief Retrieve the x component */
double util::point::x() const {
  return data[0];
}

/** @brief Retrieve the y component*/
double util::point::y() const {
  return data[1];
}

/** @brief Retrieve the z component */
double util::point::z() const {
  return data[2];
}

/** @brief Set the x component
    @param val The value to set x to
*/
void util::point::x(double val) {
  data[0] = val;
}

/** @brief Set the y component
    @param val The value to set y to
*/
void util::point::y(double val) {
  data[1] = val;
}

/** @brief Set the x component
    @param val The value to set y to
*/
void util::point::z(double val) {
  data[2] = val;
}

/** @brief Set all the components
    @param x The value to set x to
    @param y The value to set y to
    @param z The value to set z to
*/
void util::point::set(double x, double y, double z) {
  data[0] = x;
  data[1] = y;
  data[2] = z;
}

/** @brief Set all the components to zero */
void util::point::zero() {
  data[0] = 0.0;
  data[1] = 0.0;
  data[2] = 0.0;
}

/** @brief Retrive the components as a C-style array

    This method is very useful for passing the point
    data to an OpenGL gl*3dv call.  For example,
    glVertex3dv(p.c_data())
*/
const double* util::point::c_data() const {
  return data;
}

/** @brief The assignment operator
    @param p The point to copy
*/
util::point& util::point::operator=(const util::point& p) {
  data[0] = p.data[0];
  data[1] = p.data[1];
  data[2] = p.data[2];

  return *this;
}

/** @brief The addition-assignment operator
    @param p The point to add
*/
util::point& util::point::operator+=(const util::point& p) {
  data[0] += p.data[0];
  data[1] += p.data[1];
  data[2] += p.data[2];

  return *this;
}

/** @brief The subtraction-assignment operator
    @param p The point to subtract
*/
util::point& util::point::operator-=(const util::point& p) {
  data[0] -= p.data[0];
  data[1] -= p.data[1];
  data[2] -= p.data[2];

  return *this;
}

/** @brief The multiply-assignment operator
    @param s The value to multiply by
*/
util::point& util::point::operator*=(double s) {
  data[0] *= s;
  data[1] *= s;
  data[2] *= s;

  return *this;
}

/** @brief The divide-assignment operator
    @param s The value to divide by
*/
util::point& util::point::operator/=(double s) {
  data[0] /= s;
  data[1] /= s;
  data[2] /= s;

  return *this;
}

/** @brief Resize the point to the unit length */
void util::point::normalise() {
  double l = length();

  data[0] /= l;
  data[1] /= l;
  data[2] /= l;
}

/** @brief Resize the point to a given length
    @param len The length to resize the point to
*/
void util::point::resize(double len) {
  double l = length() * len;

  data[0] /= l;
  data[1] /= l;
  data[2] /= l;
}

/** @brief Find the length of the point */
double util::point::length() const {
  return std::sqrt(length_sq());
}

/** @brief Find the square of the length of the point */
double util::point::length_sq() const {
  return data[0] * data[0] + data[1] * data[1] + data[2] * data[2];
}

/** @brief Add to points
    @param a The first point
    @param b The second point
*/
util::point operator+(const util::point& a, const util::point& b) {
  return util::point(a.x() + b.x(), a.y() + b.y(), a.z() + b.z());
}

/** @brief Subtract to points
    @param a The first point
    @param b The second point
*/
util::point operator-(const util::point& a, const util::point& b) {
  return util::point(a.x() - b.x(), a.y() - b.y(), a.z() - b.z());
}

/** @brief Scalar multiplication (left)
    @param s The scalar component
    @param p The point to scale
*/
util::point operator*(double s, const util::point& p) {
  return util::point(p.x() * s, p.y() * s, p.z() * s);
}

/** @brief Scalar multiplication (right)
    @param p The point to scale
    @param s The scalar component
*/
util::point operator*(const util::point& p, double s) {
  return util::point(p.x() * s, p.y() * s, p.z() * s);
}

/** @brief Scalar division (left)
    @param s The scalar component
    @param p The point to scale
*/
util::point operator/(double s, const util::point& p) {
  return util::point(p.x() / s, p.y() / s, p.z() / s);
}

/** @brief Scalar division (right)
    @param p The point to scale
    @param s The scalar component
*/
util::point operator/(const util::point& p, double s) {
  return util::point(p.x() / s, p.y() / s, p.z() / s);
}

/** @brief Equality test
    @param a The first point
    @param b The second point
*/
bool operator==(const util::point& a, const util::point& b) {
  return
    a.x() == b.x() &&
    a.y() == b.y() &&
    a.z() == b.z();
}

/** @brief Inequality test
    @param a The first point
    @param b The second point
*/
bool operator!=(const util::point& a, const util::point& b) {
  return
    a.x() != b.x() ||
    a.y() != b.y() ||
    a.z() != b.z();
}

/** @brief Dot product
    @param a The first point
    @param b The second point
*/
double operator*(const util::point& a, const util::point& b) {
  return
    a.x() * b.x() +
    a.y() * b.y() +
    a.z() * b.z();
}

/** @brief Cross product
    @param a The first point
    @param b The second point
*/
util::point cross(const util::point& a, const util::point& b) {
  return util::point(
    a.y() * b.z() - a.z() * b.y(),
    a.z() * b.x() - a.x() * b.z(),
    a.x() * b.y() - a.y() * b.x()
  );
}

/** @brief Projection
    @param a The point to project onto
    @param b The point that is projected
*/
util::point proj(const util::point& a, const util::point& b) {
  return ((a * b) / b.length_sq()) * b;
}

/** @brief Get the length of the projection
    @param a The point to project onto
    @param b The point that is projected
*/
double proj_length(const util::point& a, const util::point& b) {
  return std::fabs(a * b) / b.length();
}

/** @brief Find the distance between two points
    @param a The first point
    @param b The second point
*/
double distance(const util::point& a, const util::point& b) {
  return std::sqrt(distance_sq(a, b));
}

/** @brief Find the square of the distance between two points
    @param a The first point
    @param b The second point
*/
double distance_sq(const util::point& a, const util::point& b) {
  return
    (a.x() - b.x()) * (a.x() - b.x()) +
    (a.y() - b.y()) * (a.y() - b.y()) +
    (a.z() - b.z()) * (a.z() - b.z());
}

/** @brief The input streaming operator
    @param is The input stream
    @param p  The point to stream into

    The text format is:
      x y z
    where x, y and z are doubles and an arbitrary whitespace
    is between each component.
*/
std::istream& operator>>(std::istream& is, util::point& p) {
  double x = double(), y = double(), z = double();
  is >> std::ws >> x >> std::ws >> y >> std::ws >> z;
  p.set(x, y, z);
  return is;
}

/** @brief The output streaming operator
    @param os The output stream
    @param p  The point to stream into

    The text format is:
      x y z
    where x, y and z are doubles and a single space is
    out put between each component.
*/
std::ostream& operator<<(std::ostream& os, const util::point& p) {
  os << p.x() << " " << p.y() << " " << p.z();
  return os;
}
